#include "Encoder.h"

volatile int Get_Encoder_CountA = 0;
volatile int Get_Encoder_CountD = 0;

EncoderData_Type EncoderData;   //编码器数据
EncoderData_Type *pEncoderData = &EncoderData;  //定义结构体指针变量

Car_Type CarData;   //小车数据
Car_Type *pCarData = &CarData;  //小车数据

void Encoder_Init(void)
{
    // 启用中断
    NVIC_ClearPendingIRQ(Encoder_GPIOA_INT_IRQN);
    NVIC_ClearPendingIRQ(Encoder_GPIOB_INT_IRQN);
    NVIC_EnableIRQ(Encoder_GPIOA_INT_IRQN);
    NVIC_EnableIRQ(Encoder_GPIOB_INT_IRQN);
}

void Encoder_Caclulate(void)        //间隔指定时间计算编码器差值
{
    pEncoderData->A_Now_Data = Get_Encoder_CountA;
    pEncoderData->D_Now_Data = Get_Encoder_CountD;

    pEncoderData->A_Err_Data = pEncoderData->A_Now_Data - pEncoderData->A_Last_Data;
    pEncoderData->D_Err_Data = pEncoderData->D_Now_Data - pEncoderData->D_Last_Data;

    pEncoderData->A_Last_Data = pEncoderData->A_Now_Data;
    pEncoderData->D_Last_Data = pEncoderData->D_Now_Data;
}

//小车数据
void Car_Data_Caclulate(void)      //计算小车速度 
{
    float Temp_DataA_CM, Temp_DataD_CM;

    // 每个间隔时间轮子转了多少厘米
    Temp_DataA_CM = (MECANUM_CIRCLE_CM / ENCODER_CIRCLE_PILSE) * pEncoderData->A_Err_Data; 
    Temp_DataD_CM = (MECANUM_CIRCLE_CM / ENCODER_CIRCLE_PILSE) * pEncoderData->D_Err_Data;

    // 左右轮子速度计算 cm/s
    CarData.Left_Speed = Temp_DataA_CM / Sampling_Interval_S; 
    CarData.Right_Speed = Temp_DataD_CM / Sampling_Interval_S;

    // 左右轮子行驶距离 cm
    CarData.Left_Distance = (MECANUM_CIRCLE_CM / ENCODER_CIRCLE_PILSE) * (float)Get_Encoder_CountA;
    CarData.Right_Distance = (MECANUM_CIRCLE_CM / ENCODER_CIRCLE_PILSE) * (float)Get_Encoder_CountD;

    // 小车行驶速度和距离
    CarData.Car_Speed = (CarData.Right_Speed + CarData.Left_Speed) / 2;
    CarData.Car_Distance = (CarData.Left_Distance + CarData.Right_Distance) / 2;

    // 小车 X 和 Y 方向速度
    CarData.Car_X_Speed = (CarData.Right_Speed + CarData.Left_Speed) / 2;
    CarData.Car_Y_Speed = (CarData.Right_Speed - CarData.Left_Speed) / 2;
}

// 2 倍精度解码 一圈为 520 个计数
void GROUP1_IRQHandler(void)
{
    switch( DL_Interrupt_getPendingGroup(DL_INTERRUPT_GROUP_1) )
    {
        // 处理右轮（P1和D1）
        case Encoder_GPIOA_INT_IIDX: // P1中断事件
            if (DL_GPIO_readPins(Encoder_d1_PORT,Encoder_d1_PIN) == 0) // 读取D1引脚状态，判断方向
            {
                Get_Encoder_CountD--; // 右轮正转
            }
            else
            {
                Get_Encoder_CountD++; // 右轮反转
            }
            break;

        // 处理左轮（P2和D2）
        case Encoder_GPIOB_INT_IIDX: // P2中断事件
            if (DL_GPIO_readPins(Encoder_d2_PORT,Encoder_d2_PIN) == 0) // 读取D2引脚状态，判断方向 
            {
                Get_Encoder_CountA++; // 左轮正转
            }
            else
            {
                Get_Encoder_CountA--; // 左轮反转
            }
            break;

    //     // 其他中断事件处理...
    }

    // 清除中断标志位（如果需要）
    // DL_Interrupt_ClearPendingGroup(DL_INTERRUPT_GROUP_1);
}